

#include "HeaderFiles.h"

uint8_t tofcmd_data[6][8] = {{0x01, 0x03, 0x00, 0x10, 0x00, 0x01, 0x85, 0xCF},
							 {0x02, 0x03, 0x00, 0x10, 0x00, 0x01, 0x85, 0xFC},
							 {0x03, 0x03, 0x00, 0x10, 0x00, 0x01, 0x84, 0x2D},
							 {0x04, 0x03, 0x00, 0x10, 0x00, 0x01, 0x85, 0x9A},
							 {0x05, 0x03, 0x00, 0x10, 0x00, 0x01, 0x84, 0x4B},
							 {0x06, 0x03, 0x00, 0x10, 0x00, 0x01, 0x84, 0x7B}};

uint8_t tofrange_buf[TOF_DEVICE_ALL][10] = {0};

uint16_t tofrange_mm[TOF_DEVICE_ALL] = {0};

STR_TOF_REG_RETURN tofX1frame = {.device_ID = TOF_DEVICE_ID_X1, .databuf = tofrange_buf[0]};
STR_TOF_REG_RETURN tofX2frame = {.device_ID = TOF_DEVICE_ID_X2, .databuf = tofrange_buf[1]};
STR_TOF_REG_RETURN tofX3frame = {.device_ID = TOF_DEVICE_ID_X3, .databuf = tofrange_buf[2]};
STR_TOF_REG_RETURN tofX4frame = {.device_ID = TOF_DEVICE_ID_X4, .databuf = tofrange_buf[3]};
STR_TOF_REG_RETURN tofX5frame = {.device_ID = TOF_DEVICE_ID_X5, .databuf = tofrange_buf[4]};
STR_TOF_REG_RETURN tofX6frame = {.device_ID = TOF_DEVICE_ID_X6, .databuf = tofrange_buf[5]};


STR_TOF_REG_RETURN *tofHdl[TOF_DEVICE_ALL]= {&tofX1frame, &tofX2frame, &tofX3frame, &tofX4frame, &tofX5frame};
											 
/*!
    \brief
    \param[in]  none
    \param[out] none
    \retval     none
*/
											 
uint16_t tof050f_range_result(uint8_t *data)
{
	uint16_t range = 0;
	
	range = (*data << 8) + *(data + 1);
	
	if(range > TOF_RANGE_MAX_MM)
	{
		range = TOF_RANGE_MAX_MM;
	}
	
	return range;
}


/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/

uint8_t tof050f_data_analysis(STR_RINGBUF *buff, STR_TOF_REG_RETURN *hdl[TOF_DEVICE_ALL])
{
	static uint8_t databuf[256] = {0};
	
	uint16_t length = 0;
	uint8_t  reslt = 0;

	length = ringBufRead(buff, (char*)databuf, 256);

	for(uint16_t i=0;i<length;i++)
	{
		if((databuf[i+1] == MODBUS_CODE_READ_REG) &&    \
		   ((databuf[i] <= TOF_DEVICE_ALL)))
		{	
			uint8_t device_num = databuf[i]-1;
			uint8_t data_length = databuf[i+2];
		    uint8_t crc_cal_length = data_length+3;
			uint8_t crc_H = databuf[i+3+data_length+1];
			uint8_t crc_L = databuf[i+3+data_length];
			
			if(checkCRC(&databuf[i], crc_cal_length, ((crc_H << 8) | (crc_L))))
			{
				hdl[device_num]->device_ID     = databuf[i];
				hdl[device_num]->function_code = databuf[i+1];
				hdl[device_num]->length        = data_length;
				hdl[device_num]->CRC_L         = crc_L;
				hdl[device_num]->CRC_H         = crc_H;
				
				for(uint8_t j=0;j<hdl[device_num]->length;j++)
				{
					hdl[device_num]->databuf[j] = databuf[i+3+j];
				}
				
				hdl[device_num]->updata_cnt++;
				
				reslt = 1;
			}
		}
	}
	
	return reslt;
}



/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/

void tof050f_test(void)
{
	static uint16_t tofcmd_num = 0;
	
	if(tofcmd_num == 1)
	{
		ringBufWrite(TOF050_TRA_Buff, (char *)&tofcmd_data[1][0], 8);
		tofcmd_num = 0;
	}
	else
	{
		ringBufWrite(TOF050_TRA_Buff, (char *)&tofcmd_data[0][0], 8);
		tofcmd_num = 1;
	}
		
	tof050f_data_analysis(TOF050_TRA_Buff, tofHdl);
	
	if(tofHdl[0]->updata_cnt != 0)
	{
		tofrange_mm[0] = tof050f_range_result(tofHdl[0]->databuf);
		#if 0
		debug_PrintfMesg("wallx1 = %d\r\n", tofrange_mm[0]);
		#endif
		tofHdl[0]->updata_cnt = 0;
	}
	
	if(tofHdl[1]->updata_cnt != 0)
	{
		tofrange_mm[1] = tof050f_range_result(tofHdl[1]->databuf);
		debug_PrintfMesg("wallx1 = %d, ", tofrange_mm[0]);
		debug_PrintfMesg("wallx2 = %d\r\n", tofrange_mm[1]);
		tofHdl[1]->updata_cnt = 0;
	}
}


/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/

void tof050f_modbus_uartsend(uint8_t num)
{	
	ringBufWrite(TOF050_TRA_Buff, (char *)&tofcmd_data[num][0], 8);
	
	tof050f_data_analysis(TOF050_REC_Buff, tofHdl);
	
	
	for(uint8_t i=0;i<TOF_DEVICE_ALL;i++)
	{
		if(tofHdl[i]->updata_cnt != 0)
		{
			tofrange_mm[i] = tof050f_range_result(tofHdl[i]->databuf);
			tofHdl[i]->updata_cnt = 0;
		}
	}
	
	#if 0
	if(tofHdl[0]->updata_cnt != 0)
	{
		tofrange_mm[0] = tof050f_range_result(tofHdl[0]->databuf);
		tofHdl[0]->updata_cnt = 0;
	}
	
	if(tofHdl[1]->updata_cnt != 0)
	{
		tofrange_mm[1] = tof050f_range_result(tofHdl[1]->databuf);
		tofHdl[1]->updata_cnt = 0;
	}
	#endif
		
}


/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/

void tof050f_modbus_comm(void)
{
	static uint8_t send_cnt = 0;		

	tof050f_modbus_uartsend(send_cnt);
	
	send_cnt++;
	send_cnt %= TOF_DEVICE_ALL;
}







